"""
PID控制器实现
包含PID控制器类和相关函数
"""

import time


class PID:
    """PID控制器类"""

    def __init__(self, P=0.2, I=0.0, D=0.0):
        """
        初始化PID控制器

        Args:
            P: 比例系数
            I: 积分系数
            D: 微分系数
        """
        self.Kp = P  # 比例增益
        self.Ki = I  # 积分增益
        self.Kd = D  # 微分增益

        self.sample_time = 0.00  # 采样时间
        self.current_time = time.time()  # 当前时间
        self.last_time = self.current_time  # 上次更新时间

        self.clear()  # 清除PID计算值

    def clear(self):
        """清除PID计算值和系数"""
        self.SetPoint = 0.0  # 设定值

        self.PTerm = 0.0  # 比例项
        self.ITerm = 0.0  # 积分项
        self.DTerm = 0.0  # 微分项
        self.last_error = 0.0  # 上次误差

        # 积分限幅
        self.int_error = 0.0  # 积分误差
        self.windup_guard = 20.0  # 积分限幅值

        self.output = 0.0  # 输出值

    def update(self, feedback_value):
        """
        计算给定反馈值的PID输出值

        计算公式:
        u(t) = K_p e(t) + K_i ∫e(t)dt + K_d de/dt

        其中:
        - K_p: 比例系数
        - K_i: 积分系数
        - K_d: 微分系数
        - e(t): 误差值
        """
        error = self.SetPoint - feedback_value  # 计算误差

        self.current_time = time.time()  # 更新当前时间
        delta_time = self.current_time - self.last_time  # 计算时间差
        delta_error = error - self.last_error  # 计算误差变化率

        if delta_time >= self.sample_time:  # 如果达到采样时间
            self.PTerm = self.Kp * error  # 计算比例项
            self.ITerm += error * delta_time  # 计算积分项

            # 积分限幅
            if self.ITerm < -self.windup_guard:
                self.ITerm = -self.windup_guard
            elif self.ITerm > self.windup_guard:
                self.ITerm = self.windup_guard

            self.DTerm = 0.0  # 初始化微分项
            if delta_time > 0:
                self.DTerm = delta_error / delta_time  # 计算微分项

            # 保存本次时间和误差用于下次计算
            self.last_time = self.current_time
            self.last_error = error

            # 计算PID输出
            self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)

    def setKp(self, proportional_gain):
        """
        设置比例增益

        Args:
            proportional_gain: 比例增益值
        """
        self.Kp = proportional_gain

    def setKi(self, integral_gain):
        """
        设置积分增益

        Args:
            integral_gain: 积分增益值
        """
        self.Ki = integral_gain

    def setKd(self, derivative_gain):
        """
        设置微分增益

        Args:
            derivative_gain: 微分增益值
        """
        self.Kd = derivative_gain

    def setWindup(self, windup):
        """
        设置积分限幅值

        积分饱和是指PID反馈控制器中，当设定值发生较大变化时，
        积分项在上升过程中累积了显著的误差，导致系统超调并继续增加，
        直到累积误差被反向误差抵消。
        主要问题是过大的超调量。

        Args:
            windup: 积分限幅值
        """
        self.windup_guard = windup

    def setSampleTime(self, sample_time):
        """
        设置采样时间

        PID控制器应该在固定的时间间隔内更新。
        基于预定的采样时间，PID控制器决定是否进行计算或立即返回。

        Args:
            sample_time: 采样时间
        """
        self.sample_time = sample_time


if __name__ == "__main__":
    # 测试代码
    x_pid = PID(P=0.2, I=0, D=0)  # 创建PID控制器实例
    x_pid.SetPoint = 5  # 设置目标值
    x_pid.update(10)  # 更新PID计算
    out = x_pid.output  # 获取输出值
    print(out)  # 打印输出值
